//#include <boost/bind.hpp>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Twist.h>
#include <mobrob_plugins/gazebo_ros_mobrob.h>

#include <gazebo/Joint.hh>
#include <gazebo/Body.hh>
#include <gazebo/Geom.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/Entity.hh>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <gazebo/XMLConfig.hh>

#include <ros/time.h>

using namespace gazebo;

GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_mobrob", GazeboRosMobrob);

enum {RFZ=0, LBZ=1, LFZ=2, RBZ=3, RFY=4, LBY=5, LFY=6, RBY=7};

const static double EPS = 1e-2;

GazeboRosMobrob::GazeboRosMobrob( Entity *parent )
  : Controller(parent)
{
  this->spinner_thread_ = new boost::thread( boost::bind( &GazeboRosMobrob::spin, this) );

  my_parent_ = dynamic_cast<Model*>(parent);

  if (!my_parent_)
    gzthrow("Gazebo_ROS_mobrob controller requires a Model as its parent");

  Param::Begin(&this->parameters);
  node_namespaceP_ = new ParamT<std::string>("node_namespace","mobrob_node",1);
  wheel_rf_rot_z_joint_nameP_ = new ParamT<std::string>("wheel_rf_rot_z_joint","wheel_rf_rot_z_joint",1);
  wheel_lb_rot_z_joint_nameP_ = new ParamT<std::string>("wheel_lb_rot_z_joint","wheel_lb_rot_z_joint",1);
  wheel_lf_rot_z_joint_nameP_ = new ParamT<std::string>("wheel_lf_rot_z_joint","wheel_lf_rot_z_joint",1);
  wheel_rb_rot_z_joint_nameP_ = new ParamT<std::string>("wheel_rb_rot_z_joint","wheel_rb_rot_z_joint",1);
  wheel_rf_rot_y_joint_nameP_ = new ParamT<std::string>("wheel_rf_rot_y_joint","wheel_rf_rot_y_joint",1);
  wheel_lb_rot_y_joint_nameP_ = new ParamT<std::string>("wheel_lb_rot_y_joint","wheel_lb_rot_y_joint",1);
  wheel_lf_rot_y_joint_nameP_ = new ParamT<std::string>("wheel_lf_rot_y_joint","wheel_lf_rot_y_joint",1);
  wheel_rb_rot_y_joint_nameP_ = new ParamT<std::string>("wheel_rb_rot_y_joint","wheel_rb_rot_y_joint",1);
  base_arduino_platform_joint_nameP_ = new ParamT<std::string>("base_arduino_platform_joint","base_arduino_platform_joint",1);
  wheel_RF_offxP_ = new ParamT<float>("wheel_RF_offx",0.2,1);
  wheel_RF_offyP_ = new ParamT<float>("wheel_RF_offy",0.2,1);
  wheel_LB_offxP_ = new ParamT<float>("wheel_LB_offx",-0.2,1);
  wheel_LB_offyP_ = new ParamT<float>("wheel_LB_offy",-0.2,1);
  wheel_LF_offxP_ = new ParamT<float>("wheel_LF_offx",0.2,1);
  wheel_LF_offyP_ = new ParamT<float>("wheel_LF_offy",-0.2,1);
  wheel_RB_offxP_ = new ParamT<float>("wheel_RB_offx",-0.2,1);
  wheel_RB_offyP_ = new ParamT<float>("wheel_RB_offy",0.2,1);
  wheel_big_diamP_ = new ParamT<float>("wheel_big_diam", 0.15,1);
  wheel_small_diamP_ = new ParamT<float>("wheel_small_diam", 0.15,1);
  torqueP_Z_ = new ParamT<float>("torque_Z", 10.0, 1);
  torqueP_Y_ = new ParamT<float>("torque_Y", 1.0, 1);
  Param::End();

  wheel_speed_ = new float[8];
  wheel_speed_[RFZ] = 0.0;
  wheel_speed_[LBZ] = 0.0;
  wheel_speed_[LFZ] = 0.0;
  wheel_speed_[RBZ] = 0.0;
  wheel_speed_[RFY] = 0.0;
  wheel_speed_[LBY] = 0.0;
  wheel_speed_[LFY] = 0.0;
  wheel_speed_[RBY] = 0.0;

  wheel_pos_ = new float[8];
  wheel_pos_[RFZ] = 0.0;
  wheel_pos_[LBZ] = 0.0;
  wheel_pos_[LFZ] = 0.0;
  wheel_pos_[RBZ] = 0.0;
  wheel_pos_[RFY] = 0.0;
  wheel_pos_[LBY] = 0.0;
  wheel_pos_[LFY] = 0.0;
  wheel_pos_[RBY] = 0.0;

  set_joints_[0] = false;
  set_joints_[1] = false;
  set_joints_[2] = false;
  set_joints_[3] = false;
  set_joints_[4] = false;
  set_joints_[5] = false;
  set_joints_[6] = false;
  set_joints_[7] = false;
  set_joints_[8] = false;
  joints_[0] = NULL;
  joints_[1] = NULL;
  joints_[2] = NULL;
  joints_[3] = NULL;
  joints_[4] = NULL;
  joints_[5] = NULL;
  joints_[6] = NULL;
  joints_[7] = NULL;
  joints_[8] = NULL;

  xB_p = 0.0;
  yB_p = 0.0;
  qB_p = 0.0;
  base_xB_p = 0.0;
  base_yB_p = 0.0;
  base_qB_p = 0.0;

  odom_pose_[0] = 0.0;
  odom_pose_[1] = 0.0;
  odom_pose_[2] = 0.0;

  odom_vel_[0] = 0.0;
  odom_vel_[1] = 0.0;
  odom_vel_[2] = 0.0;


}

GazeboRosMobrob::~GazeboRosMobrob()
{
  this->spinner_thread_->join();
  delete this->spinner_thread_;
  delete [] wheel_speed_;
  delete [] wheel_pos_;
  delete node_namespaceP_;
  delete wheel_rf_rot_z_joint_nameP_;
  delete wheel_lb_rot_z_joint_nameP_;
  delete wheel_lf_rot_z_joint_nameP_;
  delete wheel_rb_rot_z_joint_nameP_;
  delete wheel_rf_rot_y_joint_nameP_;
  delete wheel_lb_rot_y_joint_nameP_;
  delete wheel_lf_rot_y_joint_nameP_;
  delete wheel_rb_rot_y_joint_nameP_;
  delete base_arduino_platform_joint_nameP_;
  delete wheel_RF_offxP_;
  delete wheel_RF_offyP_;
  delete wheel_LB_offxP_;
  delete wheel_LB_offyP_;
  delete wheel_LF_offxP_;
  delete wheel_LF_offyP_;
  delete wheel_RB_offxP_;
  delete wheel_RB_offyP_;
  delete wheel_big_diamP_;
  delete wheel_small_diamP_;
  delete torqueP_Z_;
  delete torqueP_Y_;
  delete rosnode_;
}

void GazeboRosMobrob::LoadChild( XMLConfigNode *node )
{
  node_namespaceP_->Load(node);
  wheel_rf_rot_z_joint_nameP_->Load(node);
  wheel_lb_rot_z_joint_nameP_->Load(node);
  wheel_lf_rot_z_joint_nameP_->Load(node);
  wheel_rb_rot_z_joint_nameP_->Load(node);
  wheel_rf_rot_y_joint_nameP_->Load(node);
  wheel_lb_rot_y_joint_nameP_->Load(node);
  wheel_lf_rot_y_joint_nameP_->Load(node);
  wheel_rb_rot_y_joint_nameP_->Load(node);
  base_arduino_platform_joint_nameP_->Load(node);
  wheel_RF_offxP_->Load(node);
  wheel_RF_offyP_->Load(node);
  wheel_LB_offxP_->Load(node);
  wheel_LB_offyP_->Load(node);
  wheel_LF_offxP_->Load(node);
  wheel_LF_offyP_->Load(node);
  wheel_RB_offxP_->Load(node);
  wheel_RB_offyP_->Load(node);
  wheel_big_diamP_->Load(node);
  wheel_small_diamP_->Load(node);
  torqueP_Z_->Load(node);
  torqueP_Y_->Load(node);

  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc, argv, "gazebo_mobrob", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }

  rosnode_ = new ros::NodeHandle( **node_namespaceP_ );

  cmd_vel_sub_ = rosnode_->subscribe("/cmd_vel", 1, &GazeboRosMobrob::OnCmdVel, this );

  odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom", 1);
  ICR_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom_ICR", 1);

  joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states", 1);

  js_.name.push_back( **wheel_rf_rot_z_joint_nameP_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( **wheel_lb_rot_z_joint_nameP_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( **wheel_lf_rot_z_joint_nameP_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( **wheel_rb_rot_z_joint_nameP_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

    js_.name.push_back( **wheel_rf_rot_y_joint_nameP_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( **wheel_lb_rot_y_joint_nameP_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( **wheel_lf_rot_y_joint_nameP_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( **wheel_rb_rot_y_joint_nameP_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( **base_arduino_platform_joint_nameP_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  last_cmd_vel_time_ = 0;
}

void GazeboRosMobrob::InitChild()
{

  //TODO: fix this

  joints_[RFZ] = my_parent_->GetJoint(**wheel_rf_rot_z_joint_nameP_);
  joints_[LBZ] = my_parent_->GetJoint(**wheel_lb_rot_z_joint_nameP_);
  joints_[LFZ] = my_parent_->GetJoint(**wheel_lf_rot_z_joint_nameP_);
  joints_[RBZ] = my_parent_->GetJoint(**wheel_rb_rot_z_joint_nameP_);
  joints_[RFY] = my_parent_->GetJoint(**wheel_rf_rot_y_joint_nameP_);
  joints_[LBY] = my_parent_->GetJoint(**wheel_lb_rot_y_joint_nameP_);
  joints_[LFY] = my_parent_->GetJoint(**wheel_lf_rot_y_joint_nameP_);
  joints_[RBY] = my_parent_->GetJoint(**wheel_rb_rot_y_joint_nameP_);
  joints_[8] = my_parent_->GetJoint(**base_arduino_platform_joint_nameP_);

  if (joints_[RFZ]) set_joints_[RFZ] = true;
  if (joints_[LBZ]) set_joints_[LBZ] = true;
  if (joints_[LFZ]) set_joints_[LFZ] = true;
  if (joints_[RBZ]) set_joints_[RBZ] = true;
  if (joints_[RFY]) set_joints_[RFY] = true;
  if (joints_[LBY]) set_joints_[LBY] = true;
  if (joints_[LFY]) set_joints_[LFY] = true;
  if (joints_[RBY]) set_joints_[RBY] = true;
  if (joints_[8]) set_joints_[8] = true;

}

void GazeboRosMobrob::FiniChild()
{
  rosnode_->shutdown();
}

void GazeboRosMobrob::UpdateChild()
{
  Time time_now = Simulator::Instance()->GetSimTime();
  Time step_time = time_now - prev_update_time_;
  prev_update_time_ = time_now;

  double x1, y1, x2, y2, x3, y3, x4, y4;
  double wbd, wsd;

  x1 = **(wheel_RF_offxP_);
  y1 = **(wheel_RF_offyP_);
  x2 = **(wheel_LB_offxP_);
  y2 = **(wheel_LB_offyP_);
  x3 = **(wheel_LF_offxP_);
  y3 = **(wheel_LF_offyP_);
  x4 = **(wheel_RB_offxP_);
  y4 = **(wheel_RB_offyP_);

  wbd = **(wheel_big_diamP_);
  wsd = **(wheel_small_diamP_);

  double q1, q2, q3, q4;

  /// Get the actual steer angle

  q1 = joints_[RFZ]->GetAngle(0).GetAsRadian();
  q2 = joints_[LBZ]->GetAngle(0).GetAsRadian();
  q3 = joints_[LFZ]->GetAngle(0).GetAsRadian();
  q4 = joints_[RBZ]->GetAngle(0).GetAsRadian();



  if(xB_p > EPS || yB_p > EPS || qB_p > EPS || xB_p < -EPS || yB_p < -EPS || qB_p < -EPS){
      /// Calculate only, if a command is set!

      /// Calculate the velocity of the rotating wheels (y-axis)

      wheel_speed_[RFY] = cos(q1) * xB_p - cos(q1) * qB_p * y1 + sin(q1) * yB_p + sin(q1) * qB_p * x1;
      wheel_speed_[LBY] = cos(q2) * xB_p - cos(q2) * qB_p * y2 + sin(q2) * yB_p + sin(q2) * qB_p * x2;
      wheel_speed_[LFY] = cos(q3) * xB_p - cos(q3) * qB_p * y3 + sin(q3) * yB_p + sin(q3) * qB_p * x3;
      wheel_speed_[RBY] = cos(q4) * xB_p - cos(q4) * qB_p * y4 + sin(q4) * yB_p + sin(q4) * qB_p * x4;

      /// Calculate the desired steer angle of the wheels (z-axis)

      double q1_d, q2_d, q3_d, q4_d;

      q1_d = atan2(yB_p + qB_p * x1, xB_p - qB_p * y1);
      if(q1_d > M_PI_2){
        q1_d = q1_d - M_PI;
      }
      if(q1_d < -M_PI_2){
        q1_d = q1_d + M_PI;
      }

      q2_d = atan2(yB_p + qB_p * x2, xB_p - qB_p * y2);
      if(q2_d > M_PI_2){
        q2_d = q2_d - M_PI;
      }
      if(q2_d < -M_PI_2){
        q2_d = q2_d + M_PI;
      }

      q3_d = atan2(yB_p + qB_p * x3, xB_p - qB_p * y3);
      if(q3_d > M_PI_2){
        q3_d = q3_d - M_PI;
      }
      if(q3_d < -M_PI_2){
        q3_d = q3_d + M_PI;
      }

      q4_d = atan2(yB_p + qB_p * x4, xB_p - qB_p * y4);
      if(q4_d > M_PI_2){
        q4_d = q4_d - M_PI;
      }
      if(q4_d < -M_PI_2){
        q4_d = q4_d + M_PI;
      }


      /// Calculate the velocity for the desired steer angle

      double kz = 10.0;

      wheel_speed_[RFZ] = kz * (q1_d - q1);
      wheel_speed_[LBZ] = kz * (q2_d - q2);
      wheel_speed_[LFZ] = kz * (q3_d - q3);
      wheel_speed_[RBZ] = kz * (q4_d - q4);

      /*if((q1_d - q1) > EPS || (q1_d - q1) < -EPS){
        wheel_speed_[RFY] = 0.0;
      }
      if((q2_d - q2) > EPS || (q2_d - q2) < -EPS){
        wheel_speed_[LBY] = 0.0;
      }
      if((q3_d - q3) > EPS || (q3_d - q3) < -EPS){
        wheel_speed_[LFY] = 0.0;
      }
      if((q4_d - q4) > EPS || (q4_d - q4) < -EPS){
        wheel_speed_[RBY] = 0.0;
      }*/

  }else{
      //xB_p < EPS && yB_p < EPS && qB_p < EPS
      // No commad set!
      wheel_speed_[RFZ] = 0.0;
      wheel_speed_[LBZ] = 0.0;
      wheel_speed_[LFZ] = 0.0;
      wheel_speed_[RBZ] = 0.0;
      wheel_speed_[RFY] = 0.0;
      wheel_speed_[LBY] = 0.0;
      wheel_speed_[LFY] = 0.0;
      wheel_speed_[RBY] = 0.0;

  }

  /// Get the actual speed of the wheels y-axis (LFY -> v3, RBY -> v4)

  double v3, v4;

  v3 = joints_[LFY]->GetVelocity(0) * (wbd/2.0);
  v4 = joints_[RBY]->GetVelocity(0) * (wbd/2.0);

  /// Filter for the velocity

  if(v3 < EPS && v3 > -EPS){
    v3 = 0.0;
  }

  if(v4 < EPS && v4 > -EPS){
    v4 = 0.0;
  }

  /// Compute the base_vel with the speed of the LFY joint (v3)
  /// Compute the base_vel with the speed of the RBY joint (v4)

  double base_qB_p3, base_xB_p3, base_yB_p3;
  double base_qB_p4, base_xB_p4, base_yB_p4;
  double r3 = 0.0;
  double r4 = 0.0;

  if((q3 - q4) < EPS && (q3 - q4) > -EPS){
    // the mobrob is only driving a simple rotation or a translation
    if((v3 - v4) < EPS && (v3 - v4) > -EPS){
        //the mobrob is only drinving a translation
        r3 = 100; //far away, just for vizualization
        r4 = 100; //far away, just for vizualization
        base_qB_p3 = 0.0;
        base_qB_p4 = 0.0;
        base_xB_p3 = v3 * cos(q3);
        base_yB_p3 = v3 * sin(q3);
        base_xB_p4 = v4 * cos(q4);
        base_yB_p4 = v4 * sin(q4);
    }else{
        //the mobrob is only driving a rotaion
        r3 =  -sqrt(x3*x3 + y3*y3);
        r4 =  sqrt(x4*x4 + y4*y4);
        base_qB_p3 = v3 / r3;
        base_qB_p4 = v4 / r4;
        base_xB_p3 = 0.0;
        base_yB_p3 = 0.0;
        base_xB_p4 = 0.0;
        base_yB_p4 = 0.0;
    }
  }else{
      //the mobrob is driving a combination of rotation and translation
      r3 = (-sin(q4) * y4 + sin(q4) * y3 - cos(q4) * x4 + x3 * cos(q4)) / sin(-q4 + q3);
      base_qB_p3 = v3 / r3;
      base_xB_p3 = v3 / r3 * (y3 + r3 * cos(q3));
      base_yB_p3 = v3 / r3 * (x3 - r3 * sin(q3));

      r4 = 0.1e1 / sin(-q4 + q3) * (-y4 * sin(q3) + sin(q3) * y3 - cos(q3) * x4 + cos(q3) * x3);
      base_qB_p4 = v4 / r4;
      base_xB_p4 = v4 / r4 * (y4 + r4 * cos(q4));
      base_yB_p4 = v4 / r4 * (x4 - r4 * sin(q4));

  }

  /// average the two calculated base speed

  base_qB_p = (base_qB_p3 + base_qB_p4)/2;
  base_xB_p = (base_xB_p3 + base_xB_p4)/2;
  base_yB_p = (base_yB_p3 + base_yB_p4)/2;

  /// Compute odometric pose with the actual speed of the wheels

  odom_pose_[2] += base_qB_p * step_time.Double();
  odom_pose_[0] += step_time.Double() * (base_xB_p *  cos(odom_pose_[2]) - base_yB_p *  sin(odom_pose_[2]));
  odom_pose_[1] += step_time.Double() * (base_xB_p *  sin(odom_pose_[2]) + base_yB_p *  cos(odom_pose_[2]));


  /// Set the speed of all joints

  if (set_joints_[RFZ])
  {
    joints_[RFZ]->SetVelocity( 0, wheel_speed_[RFZ] );
    joints_[RFZ]->SetMaxForce( 0, **(torqueP_Z_) );
  }
  if (set_joints_[LBZ])
  {
    joints_[LBZ]->SetVelocity( 0, wheel_speed_[LBZ] );
    joints_[LBZ]->SetMaxForce( 0, **(torqueP_Z_) );
  }
  if (set_joints_[LFZ])
  {
    joints_[LFZ]->SetVelocity( 0, wheel_speed_[LFZ] );
    joints_[LFZ]->SetMaxForce( 0, **(torqueP_Z_) );
  }
  if (set_joints_[RBZ])
  {
    joints_[RBZ]->SetVelocity( 0, wheel_speed_[RBZ] );
    joints_[RBZ]->SetMaxForce( 0, **(torqueP_Z_) );
  }

  if (set_joints_[RFY])
  {
    joints_[RFY]->SetVelocity( 0, wheel_speed_[RFY] / (wsd/2.0) );
    joints_[RFY]->SetMaxForce( 0, **(torqueP_Y_) );
  }
  if (set_joints_[LBY])
  {
    joints_[LBY]->SetVelocity( 0, wheel_speed_[LBY] / (wsd/2.0) );
    joints_[LBY]->SetMaxForce( 0, **(torqueP_Y_) );
  }
  if (set_joints_[LFY])
  {
    joints_[LFY]->SetVelocity( 0, wheel_speed_[LFY] / (wbd/2.0) );
    joints_[LFY]->SetMaxForce( 0, **(torqueP_Y_) );
  }
  if (set_joints_[RBY])
  {
    joints_[RBY]->SetVelocity( 0, wheel_speed_[RBY] / (wbd/2.0) );
    joints_[RBY]->SetMaxForce( 0, **(torqueP_Y_) );
  }


  odom_vel_[0] = base_xB_p;
  odom_vel_[1] = base_yB_p;
  odom_vel_[2] = base_qB_p;

  nav_msgs::Odometry odom;
  odom.header.stamp.sec = time_now.sec;
  odom.header.stamp.nsec = time_now.nsec;
  odom.header.frame_id = "odom";
  odom.child_frame_id = "base_footprint";
  odom.pose.pose.position.x = odom_pose_[0];
  odom.pose.pose.position.y = odom_pose_[1];
  odom.pose.pose.position.z = 0.0;

  btQuaternion qt;
  qt.setRPY(0,0,odom_pose_[2]);

  odom.pose.pose.orientation.x = qt.getX();
  odom.pose.pose.orientation.y = qt.getY();
  odom.pose.pose.orientation.z = qt.getZ();
  odom.pose.pose.orientation.w = qt.getW();

  double pose_cov[36] = { 1e-3, 0, 0, 0, 0, 0,
                          0, 1e-3, 0, 0, 0, 0,
                          0, 0, 1e6, 0, 0, 0,
                          0, 0, 0, 1e6, 0, 0,
                          0, 0, 0, 0, 1e6, 0,
                          0, 0, 0, 0, 0, 1e3};

  memcpy( &odom.pose.covariance[0], pose_cov, sizeof(double)*36 );
  memcpy( &odom.twist.covariance[0], pose_cov, sizeof(double)*36 );

  odom.twist.twist.linear.x = odom_vel_[0];
  odom.twist.twist.linear.y = odom_vel_[1];
  odom.twist.twist.linear.z = 0;

  odom.twist.twist.angular.x = 0.0;
  odom.twist.twist.angular.y = 0.0;
  odom.twist.twist.angular.z = odom_vel_[2];

  odom_pub_.publish( odom );



  nav_msgs::Odometry odom_ICR;
  odom_ICR.header.stamp.sec = time_now.sec;
  odom_ICR.header.stamp.nsec = time_now.nsec;
  odom_ICR.header.frame_id = "odom_ICR";
  odom_ICR.child_frame_id = "base_footprint";
  odom_ICR.pose.pose.position.x = -(x4 - r4 * sin(q4));
  odom_ICR.pose.pose.position.y = -(y4 + r4 * cos(q4));
  odom_ICR.pose.pose.position.z = 0.0;

  btQuaternion qt_ICR;
  qt_ICR.setRPY(0,0,0);

  odom_ICR.pose.pose.orientation.x = qt_ICR.getX();
  odom_ICR.pose.pose.orientation.y = qt_ICR.getY();
  odom_ICR.pose.pose.orientation.z = qt_ICR.getZ();
  odom_ICR.pose.pose.orientation.w = qt_ICR.getW();

  memcpy( &odom_ICR.pose.covariance[0], pose_cov, sizeof(double)*36 );
  memcpy( &odom_ICR.twist.covariance[0], pose_cov, sizeof(double)*36 );

  odom_ICR.twist.twist.linear.x = 0;
  odom_ICR.twist.twist.linear.y = 0;
  odom_ICR.twist.twist.linear.z = 0;

  odom_ICR.twist.twist.angular.x = 0.0;
  odom_ICR.twist.twist.angular.y = 0.0;
  odom_ICR.twist.twist.angular.z = 0;

  ICR_pub_.publish( odom_ICR );

  base_xB_p = xB_p;
  base_yB_p = yB_p;
  base_qB_p = qB_p;

  js_.header.stamp.sec = time_now.sec;
  js_.header.stamp.nsec = time_now.nsec;

  /// write the velocity and angels to the joint state publisher
  if (this->set_joints_[RFZ])
  {
    js_.position[RFZ] = joints_[RFZ]->GetAngle(0).GetAsRadian();
    js_.velocity[RFZ] = joints_[RFZ]->GetVelocity(0);
  }

  if (this->set_joints_[LBZ])
  {
    js_.position[LBZ] = joints_[LBZ]->GetAngle(0).GetAsRadian();
    js_.velocity[LBZ] = joints_[LBZ]->GetVelocity(0);
  }

  if (this->set_joints_[LFZ])
  {
    js_.position[LFZ] = joints_[LFZ]->GetAngle(0).GetAsRadian();
    js_.velocity[LFZ] = joints_[LFZ]->GetVelocity(0);
  }

  if (this->set_joints_[RBZ])
  {
    js_.position[RBZ] = joints_[RBZ]->GetAngle(0).GetAsRadian();
    js_.velocity[RBZ] = joints_[RBZ]->GetVelocity(0);
  }

    if (this->set_joints_[RFY])
  {
    js_.position[RFY] = joints_[RFY]->GetAngle(0).GetAsRadian();
    js_.velocity[RFY] = joints_[RFY]->GetVelocity(0);
  }

  if (this->set_joints_[LBY])
  {
    js_.position[LBY] = joints_[LBY]->GetAngle(0).GetAsRadian();
    js_.velocity[LBY] = joints_[LBY]->GetVelocity(0);
  }

  if (this->set_joints_[LFY])
  {
    js_.position[LFY] = joints_[LFY]->GetAngle(0).GetAsRadian();
    js_.velocity[LFY] = joints_[LFY]->GetVelocity(0);
  }

  if (this->set_joints_[RBY])
  {
    js_.position[RBY] = joints_[RBY]->GetAngle(0).GetAsRadian();
    js_.velocity[RBY] = joints_[RBY]->GetVelocity(0);
  }



  /// Calculation arduino board

  double q_a, q_ad, q_a_p;
  /// max steering velocity of the arduino board
  double q_p_max = 1; // rad/sec
  double q_a_max = M_PI_4; //rad
  ///Position to calculate the stear angle of the mobrob
  double x = 0.3;
  double y = 0.0;

  q_a = joints_[8]->GetAngle(0).GetAsRadian();

  if(xB_p > EPS || yB_p > EPS || qB_p > EPS || xB_p < -EPS || yB_p < -EPS || qB_p < -EPS){
      	/// Calculate only, if a command is set!
      	q_ad = atan2(yB_p + qB_p * x, xB_p - qB_p * y);
      	//only angles between -90 and +90 degree are allowed
      	if(q_ad > M_PI_2 + 0.1){
      	  q_ad = q_ad - M_PI;
      	}
      	if(q_ad < -M_PI_2 - 0.1){
      	  q_ad = q_ad + M_PI;
     	 }
     	 
     	 if(q_ad > q_a_max){
     	   q_ad = q_a_max;
     	 }
     	 if(q_ad < -q_a_max){
     	   q_ad = -q_a_max;
     	 }
   }


  //calculate the steering velocity for the arduino board
    q_a_p = 10*(q_ad - q_a);
    if(q_a_p > q_p_max){
      q_a_p = q_p_max;
    }
    if(q_a_p < -q_p_max){
      q_a_p = -q_p_max;
    }

  if (set_joints_[8])
  {
    joints_[8]->SetVelocity( 0, q_a_p );
    joints_[8]->SetMaxForce( 0, **(torqueP_Y_) );
  }
  if (this->set_joints_[8])
  {
    js_.position[8] = joints_[8]->GetAngle(0).GetAsRadian();
    js_.velocity[8] = joints_[8]->GetVelocity(0);
  }

  joint_state_pub_.publish( js_ );


  /// timeout if didn't receive cmd in a while
  Time time_since_last_cmd = time_now - last_cmd_vel_time_;
  if (time_since_last_cmd.Double() > 0.6)
  {
    wheel_speed_[RFZ] = 0.0;
    wheel_speed_[LBZ] = 0.0;
    wheel_speed_[LFZ] = 0.0;
    wheel_speed_[RBZ] = 0.0;
    wheel_speed_[RFY] = 0.0;
    wheel_speed_[LBY] = 0.0;
    wheel_speed_[LFY] = 0.0;
    wheel_speed_[RBY] = 0.0;
    xB_p = 0.0;
    yB_p = 0.0;
    qB_p = 0.0;
  }

}

void GazeboRosMobrob::OnCmdVel( const geometry_msgs::TwistConstPtr &msg)
{
  last_cmd_vel_time_ = Simulator::Instance()->GetSimTime();

  xB_p = msg->linear.x;
  yB_p = msg->linear.y;
  qB_p = msg->angular.z;
}

void GazeboRosMobrob::spin()
{
  while(ros::ok()) ros::spinOnce();
}

